/**
 * ulog parser
 * ref: https://github.com/PX4/PX4-Autopilot/tree/master/src/modules/replay
 */
#pragma once

#include <fstream>
#include <iostream>
#include <map>
#include <sstream>
#include <vector>

#include "glog/logging.h"

#include "Eigen/Core"
#include "Eigen/Dense"

// ulog msg definition
#include "ulog_messages.h"

class ULogParser {
 public:
  enum class SensorType { ACCEL, GYRO, MAG, BARO, COMBINED, GPS };
  struct type_idx_s {
    SensorType type;
    size_t idx;
  };
  struct sensor_gps_s {
    uint64_t timestamp;
    uint64_t time_utc_usec;
    uint32_t device_id;
    int32_t lat;
    int32_t lon;
    int32_t alt;
    int32_t alt_ellipsoid;
    float s_variance_m_s;
    float c_variance_rad;
    float eph;
    float epv;
    float hdop;
    float vdop;
    int32_t noise_per_ms;
    int32_t jamming_indicator;
    float vel_m_s;
    float vel_n_m_s;
    float vel_e_m_s;
    float vel_d_m_s;
    float cog_rad;
    int32_t timestamp_time_relative;
    float heading;
    float heading_offset;
    float heading_accuracy;
    uint16_t automatic_gain_control;
    uint8_t fix_type;
    uint8_t jamming_state;
    bool vel_ned_valid;
    uint8_t satellites_used;
    uint8_t _padding0[2];  // required for logger
  };
  struct sensor_baro_s {
    uint64_t timestamp;
    uint64_t timestamp_sample;
    uint32_t device_id;
    float pressure;
    float temperature;
    uint32_t error_count;
  };
  struct sensor_combined_s {
    uint64_t timestamp;
    float gyro_rad[3];
    uint32_t gyro_integral_dt;
    int32_t accelerometer_timestamp_relative;
    float accelerometer_m_s2[3];
    uint32_t accelerometer_integral_dt;
    uint8_t accelerometer_clipping;
    uint8_t gyro_clipping;
    uint8_t accel_calibration_count;
    uint8_t gyro_calibration_count;
  };
  struct sensor_accel_s {
    uint64_t timestamp;
    uint64_t timestamp_sample;
    uint32_t device_id;
    float x;
    float y;
    float z;
    float temperature;
    uint32_t error_count;
    uint8_t clip_counter[3];
    uint8_t samples;
    uint8_t _padding0[4];  // required for logger
  };
  struct sensor_gyro_s {
    uint64_t timestamp;
    uint64_t timestamp_sample;
    uint32_t device_id;
    float x;
    float y;
    float z;
    float temperature;
    uint32_t error_count;
    uint8_t clip_counter[3];
    uint8_t samples;
    uint8_t _padding0[4];  // required for logger
  };
  struct sensor_mag_s {
    uint64_t timestamp;
    uint64_t timestamp_sample;
    uint32_t device_id;
    float x;
    float y;
    float z;
    float temperature;
    uint32_t error_count;
  };

  // construction
  ULogParser(const std::string& ulog_file = "") {
    if (ulog_file != "") {
      LOG(INFO) << "load ulog file: " << ulog_file;
      ulog_fin_.open(ulog_file, std::ios::in | std::ios::binary);
    }
  }
  ~ULogParser() {
    if (ulog_fin_.is_open()) {
      ulog_fin_.close();
    }
  }
  // API
  bool LoadULogMsgs();

 protected:
  bool ReadFileHeader(std::ifstream& fin);
  bool ReadFileDefinitions(std::ifstream& fin);
  bool ReadFormat(std::ifstream& fin, uint16_t msg_size);
  bool ReadFlagBits(std::ifstream& fin, uint16_t msg_size);
  bool ReadParams(std::ifstream& fin, uint16_t msg_size);

  bool ReadDatas(std::ifstream& fin);

  // save all sensors in sorted multimap
  template <typename T>
  void AddSensorsToMap(const std::vector<T>& sensors, const SensorType& type);

 private:
  std::ifstream ulog_fin_;
  std::vector<uint8_t> read_buffer_;

  uint64_t file_start_time_;
  std::streampos data_section_start_;

  int64_t read_until_file_position_ = 1ULL << 60;
  // all formats we read from the file
  std::map<std::string, std::string> file_formats_;

  // keep track of file position to avoid adding a subscription multiple times.
  std::streampos subscription_file_pos_ = 0;

  // save all sensors
  std::vector<sensor_accel_s> sensor_accels_;
  std::vector<sensor_gyro_s> sensor_gyros_;
  std::vector<sensor_mag_s> sensor_mags_;
  std::vector<sensor_baro_s> sensor_baros_;
  std::vector<sensor_gps_s> sensor_gpss_;
  std::vector<sensor_combined_s> sensor_combineds_;

  // all sorted sensors
  std::multimap<uint64_t, type_idx_s> all_sensors_map_;
};
